function [vB] = sensorout2in_newton(vTildeOutS, g, vB0, PSBT, KScal0, dKScalCoef, dvBiasS, dvQuantS, dvCrsDepS, StepNum, Dt) %#codegen
%SENSOROUT2IN_NEWTON 使用牛顿迭代法进行内层迭代
%
% Input Arguments
% # vTildeOutS: 3*1向量，加速度计或陀螺输出值（或对应的积分增量输出）
% # g: 3*1向量，g(w, p)值，当vTildeOutS为增量时，该值应为增量
% # vB0: 3*1向量，vB的迭代初始值，当vTildeOutS为增量时，该值应为增量
% # PSBT: 3*3矩阵，加速度计或陀螺的坐标变换矩阵
% # KScal0: 3*1向量，加速度计或陀螺的标度因数标称值
% # dKScalCoef: 3*x矩阵，x为阶数，加速度计或陀螺的标度因数误差多项式系数
% # dvBiasS, dvQuantS, dvCrsDepS: 见fgjacobian函数说明
% # StepNum：迭代步数，默认为2
% # Dt: 标量，采样周期（当vS为增量时），单位s，默认为空（瞬时值）
%
% Output Arguments
% # vB: 3*1向量，采用牛顿迭代法求得的比力或角速度
%
% References:
% # 《应用导航算法工程基础》“由输出值计算输入值的算法”

if nargin < 11
    Dt = [];
end
if nargin < 10
    StepNum = 2;
end

vB = vB0;
for i = 1:StepNum
    vS = PSBT * vB;
    [KScal, Jf, Jg] = fgjacobian_vB(vS, PSBT, KScal0, dKScalCoef, dvBiasS, dvQuantS, dvCrsDepS, Dt);
    % 计算f'Rate(vBk, p)
    fPrimeRate = Jf - Jg;
    % 计算f(vBk, p)
    f = KScal.*(vS+dvBiasS+dvQuantS);
    % 计算f'(vBk, p)
    fPrime = f+g-vTildeOutS;
    % 迭代方程
    vB = vB - fPrimeRate\fPrime;
end
end